{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "2a513698",
   "metadata": {},
   "source": [
    "# Robotics, Vision & Control 3e: for Python\n",
    "## Chapter 5: Navigation\n",
    "\n",
    "Copyright (c) 2021- Peter Corke"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6647d82a",
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "    import google.colab\n",
    "    print('Running on CoLab')\n",
    "    #!pip install roboticstoolbox-python>=1.0.2\n",
    "    !pip install git+https://github.com/petercorke/robotics-toolbox-python@future\n",
    "    !pip install spatialmath-python>=1.1.5\n",
    "    !pip install bdsim\n",
    "    !pip install --no-deps rvc3python\n",
    "    COLAB = True\n",
    "except ModuleNotFoundError:\n",
    "    COLAB = False\n",
    "\n",
    "from IPython.core.interactiveshell import InteractiveShell\n",
    "InteractiveShell.ast_node_interactivity = \"last_expr_or_assign\"\n",
    "from IPython.display import HTML\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "# add RTB examples folder to the path\n",
    "import sys, os.path\n",
    "import RVC3 as rvc\n",
    "sys.path.append(os.path.join(rvc.__path__[0], 'models'))\n",
    "\n",
    "# helper function to run bdsim in a subprocess and transfer results using a pickle file\n",
    "import pickle\n",
    "def run_shell(tool, **params):\n",
    "    global out\n",
    "    pyfile = os.path.join(rvc.__path__[0], \"models\", tool+\".py\")\n",
    "    cmd = f\"python {pyfile} -H +a -o\"\n",
    "    for key, value in params.items():\n",
    "        cmd += f' --global \"{key}={value}\"'\n",
    "    print(cmd)\n",
    "    os.system(cmd)\n",
    "    with open(\"bd.out\", \"rb\") as f:\n",
    "        out = pickle.load(f)\n",
    "        \n",
    "# ------ standard imports ------ #\n",
    "import numpy as np\n",
    "import math\n",
    "from math import pi\n",
    "np.set_printoptions(\n",
    "    linewidth=120, formatter={\n",
    "        'float': lambda x: f\"{0:8.4g}\" if abs(x) < 1e-10 else f\"{x:8.4g}\"})\n",
    "np.random.seed(0)\n",
    "from spatialmath import *\n",
    "from spatialmath.base import *\n",
    "from roboticstoolbox import *\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "14a19383",
   "metadata": {},
   "source": [
    "# 5.1 Introduction to Reactive Navigation\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5b766bc9",
   "metadata": {},
   "source": [
    "## 5.1.1 Braitenberg Vehicles\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e2ea70f5",
   "metadata": {
    "lines_to_next_cell": 1
   },
   "outputs": [],
   "source": [
    "def sensorfield(x, y):\n",
    "  xc, yc = (60, 90)\n",
    "  return 200 / ((x - xc) ** 2 + (y - yc) ** 2 + 200)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "209e615f",
   "metadata": {},
   "source": [
    "<div style=\"background-color:red\">\n",
    "<span style=\"background-color:red; font-size:20pt\">NOTE</span>\n",
    "\n",
    "There are issues in running bdsim from inside Jupyter.  We have two options, disable all graphics (which is a bit dull) or run bdsim from a subprocess with its own popout windows.\n",
    "\n",
    "For Colab we can only do the first option.  \n",
    "\n",
    "Otherwise, we use the wrapper function `run_shell()` defined in the first cell to spawn a new Python instance to run the block diagram which will pop up new windows on your screen to display a simple animation showing the vehicle's motion in the plane.  This makes it a bit harder to get parameters and data between Jupyter and the `bdsim` simulation, so:\n",
    "* parameter values are passed to `bdsim` as `--global` command line options.\n",
    "* the `-o` command line option to `bdsim` causes it to write results as a pickle file, which are then imported back into Jupyter.</div>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b112d3a5",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "  %run -m braitenberg -H -g  # no graphics\n",
    "else:\n",
    "  run_shell(\"braitenberg\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a404be12",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(out.x[:,0], out.x[:,1]);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "10f0e01f",
   "metadata": {},
   "source": [
    "## 5.1.2 Simple Automata\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ce0d6825",
   "metadata": {},
   "outputs": [],
   "source": [
    "house = rtb_load_matfile(\"data/house.mat\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c3f665f7",
   "metadata": {},
   "outputs": [],
   "source": [
    "floorplan = house[\"floorplan\"];\n",
    "floorplan.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e4e077b5",
   "metadata": {},
   "outputs": [],
   "source": [
    "places = house[\"places\"];\n",
    "places.keys()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c6fee7ef",
   "metadata": {},
   "outputs": [],
   "source": [
    "places.br3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "306d6ab1",
   "metadata": {},
   "outputs": [],
   "source": [
    "bug = Bug2(occgrid=floorplan);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "de5a3d40",
   "metadata": {},
   "outputs": [],
   "source": [
    "bug.plot();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dbad5c81",
   "metadata": {},
   "outputs": [],
   "source": [
    "# bug.run(start=places.br3, goal=places.kitchen, animate=True);  # wont work in Jupyter\n",
    "\n",
    "path = bug.run(start=places.br3, goal=places.kitchen);  # compute the path\n",
    "path.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "28389f40",
   "metadata": {},
   "outputs": [],
   "source": [
    "bug.plot(path);  # then overlay it on the map"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "52bfff8d",
   "metadata": {},
   "source": [
    "# 5.2 Introduction to Map-Based Navigation\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "24f1fe6f",
   "metadata": {},
   "source": [
    "# 5.3 Planning with a Graph-Based Map\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0a71ffbe",
   "metadata": {},
   "outputs": [],
   "source": [
    "data = rtb_load_jsonfile(\"data/queensland.json\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "47d979a0",
   "metadata": {},
   "outputs": [],
   "source": [
    "for name, info in data[\"places\"].items():\n",
    "  plot_point(info[\"utm\"], text=name)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "55359c9a",
   "metadata": {},
   "outputs": [],
   "source": [
    "data[\"routes\"][0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d378306d",
   "metadata": {},
   "outputs": [],
   "source": [
    "import pgraph\n",
    "g = pgraph.UGraph()  # create empty undirected graph\n",
    "for name, info in data[\"places\"].items():\n",
    "  g.add_vertex(name=name, coord=info[\"utm\"])  # add a place\n",
    "for route in data[\"routes\"]:\n",
    "  g.add_edge(route[\"start\"], route[\"end\"], \n",
    "             cost=route[\"distance\"])  # add a route"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c4050e27",
   "metadata": {},
   "outputs": [],
   "source": [
    "g.plot()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cad642b9",
   "metadata": {},
   "outputs": [],
   "source": [
    "g.n\n",
    "g.ne"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "aed2eb4e",
   "metadata": {},
   "outputs": [],
   "source": [
    "g[\"Brisbane\"]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "392f92df",
   "metadata": {},
   "outputs": [],
   "source": [
    "g[\"Brisbane\"].neighbors() "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "44fab7bb",
   "metadata": {},
   "outputs": [],
   "source": [
    "g[\"Brisbane\"].degree"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4a466d6f",
   "metadata": {},
   "outputs": [],
   "source": [
    "g.average_degree()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a09eb6c0",
   "metadata": {},
   "outputs": [],
   "source": [
    "g.nc"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "37b41d68",
   "metadata": {},
   "outputs": [],
   "source": [
    "edges = g[\"Brisbane\"].edges()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "200d3cee",
   "metadata": {},
   "outputs": [],
   "source": [
    "edges[0].endpoints"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f4560b84",
   "metadata": {},
   "outputs": [],
   "source": [
    "path, length = g.path_BFS(\"Hughenden\", \"Brisbane\")\n",
    "length\n",
    "\", \".join([p.name for p in path])\n",
    "ax = g.plot(block=None)\n",
    "g.highlight_path(path)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ebc5356c",
   "metadata": {},
   "outputs": [],
   "source": [
    "path, length, parents = g.path_UCS(\"Hughenden\", \"Brisbane\")\n",
    "length\n",
    "\", \".join([p.name for p in path])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "42804579",
   "metadata": {},
   "outputs": [],
   "source": [
    "parents[\"Winton\"]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b78b942c",
   "metadata": {},
   "outputs": [],
   "source": [
    "tree = pgraph.DGraph.Dict(parents);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f4508ca5",
   "metadata": {},
   "outputs": [],
   "source": [
    "tree.showgraph()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bed542b3",
   "metadata": {
    "lines_to_next_cell": 2
   },
   "outputs": [],
   "source": [
    "g.path_UCS(\"Hughenden\", \"Brisbane\", verbose=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8de0751f",
   "metadata": {
    "lines_to_next_cell": 2
   },
   "outputs": [],
   "source": [
    "g[\"Bedourie\"].edgeto(g[\"Birdsville\"]).cost"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "909343ee",
   "metadata": {},
   "outputs": [],
   "source": [
    "path, length, parents = g.path_Astar(\"Hughenden\", \"Brisbane\", summary=True)\n",
    "length\n",
    "\", \".join([p.name for p in path])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8b47835e",
   "metadata": {},
   "outputs": [],
   "source": [
    "# find the unique vertex names\n",
    "visited = set(list(parents.keys()) + list(parents.values()));\n",
    "plt.clf()\n",
    "g.plot(block=None)\n",
    "# g.highlight_vertex(visited, color=\"yellow\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "79500078",
   "metadata": {},
   "outputs": [],
   "source": [
    "g.path_Astar(\"Hughenden\", \"Brisbane\", verbose=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ee9fcf64",
   "metadata": {},
   "source": [
    "## 5.3.1 Minimum-Time Path Planning\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c35cb465",
   "metadata": {},
   "outputs": [],
   "source": [
    "g = pgraph.UGraph()\n",
    "for name, info in data[\"places\"].items():\n",
    "  g.add_vertex(name=name, coord=info[\"utm\"])\n",
    "for route in data[\"routes\"]:\n",
    "  g.add_edge(route[\"start\"], route[\"end\"], \n",
    "             cost=route[\"distance\"] / route[\"speed\"])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d2a496bb",
   "metadata": {},
   "outputs": [],
   "source": [
    "g.heuristic = lambda x: np.linalg.norm(x) / 100"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5bc14728",
   "metadata": {},
   "outputs": [],
   "source": [
    "path, time, _ = g.path_Astar(\"Hughenden\", \"Brisbane\")\n",
    "time\n",
    "\", \".join([p.name for p in path])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1c7a0296",
   "metadata": {},
   "source": [
    "# 5.4 Planning with an Occupancy-Grid Map\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3f94f372",
   "metadata": {},
   "source": [
    "## 5.4.1 Distance Transform\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9f48cc9b",
   "metadata": {},
   "outputs": [],
   "source": [
    "map = np.zeros((100, 100));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b849bf65",
   "metadata": {},
   "outputs": [],
   "source": [
    "map[40:50, 20:80] = 1;  # set to occupied"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a85db087",
   "metadata": {},
   "outputs": [],
   "source": [
    "map.ravel()[np.random.choice(map.size, 100, replace=False)] = 1;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a698f57c",
   "metadata": {},
   "outputs": [],
   "source": [
    "simplegrid = np.zeros((6, 6));\n",
    "simplegrid[2:5, 3:5] = 1\n",
    "simplegrid[3:5, 2] = 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2c84f713",
   "metadata": {},
   "outputs": [],
   "source": [
    "dx = DistanceTransformPlanner(occgrid=simplegrid);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b67ea87c",
   "metadata": {},
   "outputs": [],
   "source": [
    "dx.plan(goal=(1, 1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7bb48742",
   "metadata": {},
   "outputs": [],
   "source": [
    "dx.plot(labelvalues=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "611b1274",
   "metadata": {},
   "outputs": [],
   "source": [
    "dx = DistanceTransformPlanner(occgrid=simplegrid, distance=\"manhattan\");\n",
    "dx.plan(goal=(1, 1))\n",
    "dx.plot(labelvalues=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "aa887a86",
   "metadata": {},
   "outputs": [],
   "source": [
    "dx.plot_3d();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d4d98f7e",
   "metadata": {},
   "outputs": [],
   "source": [
    "path = dx.query(start=(5, 4))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "02b3118c",
   "metadata": {},
   "outputs": [],
   "source": [
    "dx.plot(path);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "50956b6f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# dx.query(start=(5, 4), animate=True);\n",
    "# won't work in Jupyter"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "eb8470a8",
   "metadata": {},
   "outputs": [],
   "source": [
    "house = rtb_load_matfile(\"data/house.mat\");\n",
    "floorplan = house[\"floorplan\"];\n",
    "places = house[\"places\"];"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6e208ae2",
   "metadata": {},
   "outputs": [],
   "source": [
    "dx = DistanceTransformPlanner(occgrid=floorplan);\n",
    "dx.plan(goal=places.kitchen)\n",
    "dx.plot();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "96e7f16a",
   "metadata": {},
   "outputs": [],
   "source": [
    "path = dx.query(start=places.br3);\n",
    "path.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "77ff7b14",
   "metadata": {},
   "outputs": [],
   "source": [
    "dx.plot(path);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e1c41ddf",
   "metadata": {},
   "outputs": [],
   "source": [
    "dx.plan(places.kitchen, summary=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ca282db4",
   "metadata": {},
   "outputs": [],
   "source": [
    "# dx.plan(places.kitchen, animate=True);\n",
    "# won't work in Jupyter"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9c060588",
   "metadata": {},
   "outputs": [],
   "source": [
    "dx = DistanceTransformPlanner(occgrid=floorplan, inflate=5);\n",
    "dx.plan(places.kitchen);\n",
    "p = dx.query(places.br3);\n",
    "dx.plot(p);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "27754062",
   "metadata": {},
   "source": [
    "## 5.4.2 D*\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "842acd9d",
   "metadata": {},
   "outputs": [],
   "source": [
    "dstar = DstarPlanner(occgrid=floorplan);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cb22ef8d",
   "metadata": {},
   "outputs": [],
   "source": [
    "c = dstar.costmap;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "21a96738",
   "metadata": {},
   "outputs": [],
   "source": [
    "dstar.plan(goal=places.kitchen);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ad3b48a9",
   "metadata": {},
   "outputs": [],
   "source": [
    "nexpand0 = dstar.nexpand"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3a0dcbd2",
   "metadata": {
    "lines_to_next_cell": 1
   },
   "outputs": [],
   "source": [
    "path = dstar.query(start=places.br3);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "754cadaf",
   "metadata": {
    "lines_to_next_cell": 1
   },
   "outputs": [],
   "source": [
    "def sensorfunc(pos):\n",
    "   if pos[0] == 300:  # near the door?\n",
    "       changes = []\n",
    "       for x in range(300, 325):\n",
    "           for y in range(115,125):\n",
    "               changes.append((x, y, np.inf))\n",
    "       return changes"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5b799e8b",
   "metadata": {},
   "outputs": [],
   "source": [
    "dstar.query(start=places.br3, sensor=sensorfunc);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "05364a25",
   "metadata": {},
   "outputs": [],
   "source": [
    "dstar.nexpand - nexpand0"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "246ba6fc",
   "metadata": {},
   "source": [
    "# 5.5 Planning with Roadmaps\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "029bc0a0",
   "metadata": {},
   "outputs": [],
   "source": [
    "occgrid = floorplan.copy();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8a32a31d",
   "metadata": {},
   "outputs": [],
   "source": [
    "occgrid[0, :] = 1\n",
    "occgrid[-1, :] = 1\n",
    "occgrid[:, 0] = 1\n",
    "occgrid[:, -1] = 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8b682313",
   "metadata": {},
   "outputs": [],
   "source": [
    "from machinevisiontoolbox import Image\n",
    "\n",
    "freespace = Image(occgrid == 0)\n",
    "freespace.disp();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e8c1ba73",
   "metadata": {},
   "outputs": [],
   "source": [
    "skeleton = freespace.thin().disp();"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "27b00e0e",
   "metadata": {},
   "source": [
    "### Excurse 5.8: Voronoi Tessellation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4efaa8b6",
   "metadata": {},
   "outputs": [],
   "source": [
    "sites = np.random.uniform(size=(2,9));\n",
    "from scipy.spatial import Voronoi\n",
    "\n",
    "vor = Voronoi(sites.T);\n",
    "\n",
    "from scipy.spatial import voronoi_plot_2d\n",
    "\n",
    "ax = plt.gca()\n",
    "ax.plot(sites[0,:], sites[1,:], \"k*\", markersize=10)\n",
    "voronoi_plot_2d(vor, ax=ax, show_points=False)\n",
    "ax.set_aspect('equal')\n",
    "ax.set_xlim(0, 1)\n",
    "ax.set_ylim(0, 1)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5695d1e6",
   "metadata": {},
   "outputs": [],
   "source": [
    "prm = PRMPlanner(occgrid=floorplan, seed=0);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "764f31fc",
   "metadata": {},
   "outputs": [],
   "source": [
    "prm.plan(npoints=50)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3cd10bfc",
   "metadata": {},
   "outputs": [],
   "source": [
    "prm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6b2b319c",
   "metadata": {},
   "outputs": [],
   "source": [
    "prm.plot();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0fc357c6",
   "metadata": {},
   "outputs": [],
   "source": [
    "prm.plan(npoints=300)\n",
    "prm.plot();\n",
    "prm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6ac8612c",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.random.rand(5)  # seeded by NumPy (not repeatable)\n",
    "np.random.seed(42)\n",
    "np.random.rand(5)  # with seed of 42\n",
    "np.random.seed(42)\n",
    "np.random.rand(5)  # with seed of 42"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6fdf4c5c",
   "metadata": {},
   "outputs": [],
   "source": [
    "path = prm.query(start=places.br3, goal=places.kitchen);\n",
    "prm.plot(path)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "64e7d5d6",
   "metadata": {},
   "outputs": [],
   "source": [
    "path = prm.query(start=places.br2, goal=places.kitchen);\n",
    "path.shape"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "76fe0c35",
   "metadata": {},
   "source": [
    "# 5.6 Planning Driveable Paths\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "57b8208d",
   "metadata": {},
   "outputs": [],
   "source": [
    "qs = (0, 0, pi/2);\n",
    "qg = (1, 0, pi/2);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5262b393",
   "metadata": {},
   "source": [
    "## 5.6.1 Dubins Path Planner\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bb7654ae",
   "metadata": {},
   "outputs": [],
   "source": [
    "dubins = DubinsPlanner(curvature=1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ad5d03dd",
   "metadata": {},
   "outputs": [],
   "source": [
    "path, status = dubins.query(qs, qg)\n",
    "path.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "312d2291",
   "metadata": {},
   "outputs": [],
   "source": [
    "dubins.plot(path);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f7dbdcd3",
   "metadata": {},
   "outputs": [],
   "source": [
    "status"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fb837228",
   "metadata": {},
   "outputs": [],
   "source": [
    "dubins.plot(path, configspace=True);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "72b86806",
   "metadata": {},
   "source": [
    "## 5.6.2 Reeds-Shepp Path Planner\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4c424e50",
   "metadata": {},
   "outputs": [],
   "source": [
    "rs = ReedsSheppPlanner(curvature=1)\n",
    "path, status = rs.query(qs, qg)\n",
    "path.shape\n",
    "status"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3a573ebc",
   "metadata": {},
   "outputs": [],
   "source": [
    "rs.plot(path, direction=status.direction);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5969df39",
   "metadata": {},
   "outputs": [],
   "source": [
    "rs.plot(path, configspace=True);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2859e9db",
   "metadata": {},
   "source": [
    "## 5.6.3 Lattice Planner\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "af3a1985",
   "metadata": {},
   "outputs": [],
   "source": [
    "lp = LatticePlanner();\n",
    "lp.plan(iterations=1, summary=True)\n",
    "lp.plot()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "267ef630",
   "metadata": {},
   "outputs": [],
   "source": [
    "lp.plan(iterations=2, summary=True)\n",
    "lp.plot()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c6deade8",
   "metadata": {},
   "outputs": [],
   "source": [
    "lp.plot(configspace=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8a602d81",
   "metadata": {},
   "outputs": [],
   "source": [
    "lp.plan(iterations=8, summary=True)\n",
    "lp.plot()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dba23e17",
   "metadata": {},
   "outputs": [],
   "source": [
    "path, status = lp.query(qs, qg);\n",
    "path.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "81a98e1f",
   "metadata": {},
   "outputs": [],
   "source": [
    "lp.plot(path)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b5dfde03",
   "metadata": {},
   "outputs": [],
   "source": [
    "status"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "20f33628",
   "metadata": {},
   "outputs": [],
   "source": [
    "lattice = LatticePlanner(costs=[1, 10, 1])  # S, L, R\n",
    "lp.plan(iterations=8)\n",
    "path, status = lp.query(qs, qg)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "48924cd5",
   "metadata": {},
   "outputs": [],
   "source": [
    "og = BinaryOccupancyGrid(workspace=[-5, 5, -5, 5], value=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e51c05eb",
   "metadata": {},
   "outputs": [],
   "source": [
    "og.set([-2, 0, -2, -1], True)\n",
    "og.set([2, 3, 0, 4], True)\n",
    "og.set([0, 2, -2, -2], True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "edfee0af",
   "metadata": {},
   "outputs": [],
   "source": [
    "lattice = LatticePlanner(occgrid=og)\n",
    "lattice.plan(iterations=None)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b7799048",
   "metadata": {},
   "outputs": [],
   "source": [
    "path, status = lattice.query(qs, qg)\n",
    "lattice.plot(path)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1097f00a",
   "metadata": {},
   "source": [
    "## 5.6.4 Curvature Polynomials\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c895149b",
   "metadata": {},
   "outputs": [],
   "source": [
    "cpoly = CurvaturePolyPlanner()\n",
    "path, status = cpoly.query(qs, qg)\n",
    "status\n",
    "cpoly.plot(path);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "143b785e",
   "metadata": {},
   "source": [
    "## 5.6.5 Planning in Configuration Space\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9fd7b5eb",
   "metadata": {},
   "outputs": [],
   "source": [
    "map = PolygonMap(workspace=[0, 10]);\n",
    "map.add([(5, 50), (5, 6), (6, 6), (6, 50)])\n",
    "map.add([(5, 4), (5, -50), (6, -50), (6, 4)])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6cf4b7b3",
   "metadata": {},
   "outputs": [],
   "source": [
    "map.plot()\n",
    "\n",
    "qs = (2, 8, -pi/2);\n",
    "qg = (8, 2, -pi/2);\n",
    "\n",
    "piano = VehicleIcon(\"piano\", scale=3)\n",
    "piano.plot(qs);\n",
    "piano.plot(qg);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0418cca2",
   "metadata": {},
   "outputs": [],
   "source": [
    "l, w = 3, 1.5;\n",
    "vpolygon = Polygon2([(-l/2, w/2), (-l/2, -w/2),\n",
    "                     (l/2, -w/2), (l/2, w/2)]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "35934fdd",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = (2, 4, 0);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b246be38",
   "metadata": {},
   "outputs": [],
   "source": [
    "map.iscollision(vpolygon.transformed(SE2(q)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "755684e9",
   "metadata": {},
   "outputs": [],
   "source": [
    "vehicle = Bicycle(steer_max=1, L=2, polygon=vpolygon);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "aa92d738",
   "metadata": {},
   "outputs": [],
   "source": [
    "vehicle.curvature_max"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "847436e6",
   "metadata": {},
   "outputs": [],
   "source": [
    "rrt = RRTPlanner(map=map, vehicle=vehicle, npoints=50, showsamples=True, seed=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bc3730b1",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = rrt.qrandom()\n",
    "rrt.iscollision(q)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bf9bce53",
   "metadata": {},
   "outputs": [],
   "source": [
    "# rrt.plan(goal=qg, animate=True)\n",
    "rrt.plan(goal=qg, animate=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "563ca2fd",
   "metadata": {},
   "outputs": [],
   "source": [
    "path, status = rrt.query(start=qs);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2296276d",
   "metadata": {},
   "outputs": [],
   "source": [
    "rrt.g.plot()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b40eabb0",
   "metadata": {},
   "outputs": [],
   "source": [
    "rrt.g.highlight_path(status.vertices, color=\"red\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9e34de40",
   "metadata": {},
   "outputs": [],
   "source": [
    "rrt.plot(path);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "02bec1a6",
   "metadata": {},
   "outputs": [],
   "source": [
    "status.initial_d"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1bd500ba",
   "metadata": {},
   "source": [
    "# 5.7 Advanced Topics\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9136aeca",
   "metadata": {},
   "source": [
    "## 5.7.3 Converting between Graphs and Matrices\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a41ea902",
   "metadata": {},
   "outputs": [],
   "source": [
    "import pgraph\n",
    "g = pgraph.UGraph()\n",
    "for i in range(4):  # add 4 vertices\n",
    "  g.add_vertex()\n",
    "g[0].connect(g[1], cost=1);  # 0 -- 1\n",
    "g[0].connect(g[3], cost=2);  # 0 -- 3\n",
    "g[1].connect(g[2], cost=3);  # 1 -- 2\n",
    "g.distance()"
   ]
  }
 ],
 "metadata": {
  "jupytext": {
   "cell_metadata_filter": "-all",
   "main_language": "python",
   "notebook_metadata_filter": "-all"
  },
  "kernelspec": {
   "display_name": "dev",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.9"
  },
  "vscode": {
   "interpreter": {
    "hash": "b7d6b0d76025b9176285a6442c3dd6dd39bcfe7241029b7898b7106bd5e9b472"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
